#include "LSCMBuilder.h"

namespace LSCM
{
	const float MIN_DEFAULT = FLT_MAX;
	const float MAX_DEFAULT = -FLT_MAX;
	const float THRESHOLD = 0.0000001f;

	LSCMBuilder::LSCMBuilder(Mesh* mesh, FeatureMetric* metric) : m_mesh(mesh), m_metric(metric)
	{
		m_mesh->add_property(m_vertexStates);
		m_mesh->add_property(m_faceId);
	}

	LSCMBuilder::~LSCMBuilder()
	{
		m_mesh->remove_property(m_vertexStates);
		m_mesh->remove_property(m_faceId);
	}

	std::vector<Chart> LSCMBuilder::Build()
	{
		std::vector<Chart> charts;

		// 根据是否传入度量判定是否进行特征检测
		if (m_metric != nullptr)
		{
			TIMER_START(FEATURE);
			SODFeatureMetric metric(15);
			FeatureBuilder fBuilder(m_mesh, &metric, 10, 20);
			TIMER_END(FEATURE);

			TIMER_START(CHART);
			ChartBuilder cBuilder(m_mesh, fBuilder.Build());
			charts = cBuilder.Build();
			TIMER_END(CHART);

			if (!cBuilder.Validate())
				std::cout << "Charts not validate." << std::endl;
		}

		// 如果特征检测失败，就直接将整个网格作为图表
		if (charts.empty())
		{
			charts.push_back(Chart());
			Chart& chart = charts.back();

			auto& vertices = chart.vertices();
			for (auto& face : m_mesh->faces())
				chart.add(face, 0);

			for (auto& v : m_mesh->vertices())
				vertices.push_back(v);
		}
		
		// 依次构建图表
		for (auto& chart : charts)
		{
			// 初始化标记，获得活动顶点的数量
			int act = initializeIndex(chart);

			// M 的行数等于三角形数，列数等于顶点数，是系数矩阵
			// 由于虚实分离，得到 A 矩阵的规模是 2 倍行数
			const auto numRows = chart.faces().size() * 2;
			SparseMatrix A(numRows, act * 2);
			SparseMatrix fA(numRows, (chart.vertices().size() - act) * 2);
			MatrixXx1 fx(fA.cols());

			// 填充系数，获得基本矩阵
			initializeSystem(chart, A, fA, fx);

			// Ax = b, b = -Mp * Up
			MatrixXx1 e = -fA * fx;

			// 稀疏矩阵最小二乘共轭梯度求解器
			Eigen::LeastSquaresConjugateGradient<SparseMatrix> solver;
			solver.setTolerance(THRESHOLD);

			// 设置最大迭代次数，默认是列数 * 2
			solver.setMaxIterations(A.cols());
			solver.compute(A);

			// 存储 UV 参数化
			MatrixXx1 x = solver.solve(e);
			for (const auto& vertex : chart.vertices())
			{
				// 获得顶点信息
				auto& state = m_mesh->property(m_vertexStates, vertex);

				// 固定顶点已经有参数坐标，只需要考虑活动顶点
				auto uv = Mesh::TexCoord2D();
				if (state.fixed)
					uv = m_mesh->texcoord2D(vertex);
				else
				{
					uv = Mesh::TexCoord2D(x[state.id], x[state.id + 1]);
					m_mesh->set_texcoord2D(vertex, uv);
				}
			}
		}

		return charts;
	}

	Mesh::Point LSCMBuilder::OBB(const VertexList& vertices)
	{
		int n = vertices.size();

		// 坐标分离为 3 个随机变量
		Eigen::VectorXd X(n), Y(n), Z(n);
		for (int i = 0; i < n; i++)
		{
			auto& p = m_mesh->point(vertices[i]);

			X[i] = p[0];
			Y[i] = p[1];
			Z[i] = p[2];
		}

		// 计算期望点
		double E[] = { 0, 0, 0 };
		E[0] = X.sum() / n;
		E[1] = Y.sum() / n;
		E[2] = Z.sum() / n;

		std::cout << "N:" << n << std::endl;
		std::cout << "E:" << E[0] << " " << E[1] << " " << E[2] << std::endl;

		// 计算协方差矩阵
		Eigen::MatrixXd B(3, 3);
		B.setZero();
		for (int i = 0; i < n; i++)
		{
			B(0, 0) += X[i] * X[i];
			B(0, 1) += X[i] * Y[i];
			B(0, 2) += X[i] * Z[i];

			B(1, 0) += Y[i] * X[i];
			B(1, 1) += Y[i] * Y[i];
			B(1, 2) += Y[i] * Z[i];

			B(2, 0) += Z[i] * X[i];
			B(2, 1) += Z[i] * Y[i];
			B(2, 2) += Z[i] * Z[i];
		}
		B = B / n;

		// 计算 Xi_i 和 Xi_j 的协方差
		for (int i = 0; i < 3; i++)
			for (int j = 0; j < 3; j++)
				B(i, j) -= E[i] * E[j];

		std::cout << "B:\n" << B << std::endl;

		// 特征分解
		Eigen::EigenSolver<Eigen::MatrixXd> es(B);

		// 获得特征值，取实部
		Eigen::VectorXcd cval = es.eigenvalues();
		Eigen::VectorXd val(cval.size());
		for (int i = 0; i < val.size(); i++)
			val[i] = cval[i].real();

		// 获得特征向量的矩阵，取实部
		Eigen::MatrixXcd cV = es.eigenvectors();
		Eigen::MatrixXd V(cV.rows(), cV.cols());
		for (int i = 0; i < V.rows(); i++)
			for (int j = 0; j < V.cols(); j++)
				V(i, j) = cV(i, j).real();

		std::cout << "eigens:\n" << cval << std::endl;
		std::cout << "eigenvectors:\n" << cV << std::endl;

		// 取出最大主方向
		int maxId = 0;
		double maxVal = MAX_DEFAULT;
		for (int i = 0; i < 3; i++)
		{
			if (val[i] > maxVal)
			{
				maxId = i;
				maxVal = val[i];
			}
		}

		return Mesh::Point(V(0, maxId), V(1, maxId), V(2, maxId)).normalized();
	}

	int LSCMBuilder::initializeIndex(const Chart& chart)
	{
		// 计算主方向，获得锚定点
		auto& vertices = chart.vertices();

		// 第一个图表使用最远端点作为锚定点
		Mesh::Point mainDir = OBB(vertices);

		double minProj = MIN_DEFAULT;
		double maxProj = MAX_DEFAULT;
		Mesh::VertexHandle h1, h2;
		for (auto& v : vertices)
		{
			auto& p = m_mesh->point(v);
			double proj = p | mainDir;

			if (proj < minProj)
			{
				h1 = v;
				minProj = proj;
			}

			if (proj > maxProj)
			{
				h2 = v;
				maxProj = proj;
			}
		}

		std::cout << "Dir: " << mainDir << std::endl;
		std::cout << "min: " << minProj << std::endl;
		std::cout << "max: " << maxProj << std::endl;

		// 标记活动顶点序号
		int act = 0;
		for (auto& v : vertices)
		{
			auto& state = m_mesh->property(m_vertexStates, v);
			if (v != h1 && v != h2)
			{
				state.id = act * 2;
				state.fixed = false;
				act++;
			}
		}

		// 标记锚点序号
		m_mesh->property(m_vertexStates, h1) = { true, 0 };
		m_mesh->property(m_vertexStates, h2) = { true, 2 };

		// 记录锚定点的基准参数坐标
		m_mesh->set_texcoord2D(h1, { 0, 0 });
		m_mesh->set_texcoord2D(h2, { 1, 1 });

		// 标记面对应的序号
		auto& faces = chart.faces();
		for (unsigned int i = 0; i < faces.size(); i++)
			m_mesh->property(m_faceId, faces[i]) = i * 2;

		return act;
	}

	void LSCMBuilder::initializeSystem(const Chart& chart, SparseMatrix& A, SparseMatrix& fA, MatrixXx1& fx)
	{
		// 用来初始化稀疏矩阵
		TripletList a, fa;

		// 处理每个面
		for (const auto& face : chart.faces())
		{
			// 获得面对应的 id，也就是它对应的行标。实数行标和虚数行标
			const auto realRow = m_mesh->property(m_faceId, face);
			const auto imRow = realRow + 1;

			// 计算三角面的局部坐标值存在 pv 中
			Mesh::Point pv[3];
			Mesh::VertexHandle hv[3];
			projectFace(face, pv, hv);

			const auto pv01 = pv[1] - pv[0];
			const auto pv02 = pv[2] - pv[0];

			// 设置系数的函数
			auto setCoefficient = [&](int row, const Mesh::VertexHandle& vertex, double u, double v)
			{
				// 区分固定点和活动点
				auto& state = m_mesh->property(m_vertexStates, vertex);
				auto& target = state.fixed ? fa : a;

				// 填充相邻两列，分别对应实部和虚部
				target.emplace_back(Triplet{ row, state.id, u });
				target.emplace_back(Triplet{ row, state.id + 1, v });

				// 固定点还需要设置
				if (state.fixed)
				{
					auto& tex = m_mesh->texcoord2D(vertex);
					fx[state.id] = tex[0];
					fx[state.id + 1] = tex[1];
				}
			};

			// 填充 a 系数，a 是 Triplet 存放稀疏格式 (i,j,value)
			// Real and Imaginary
			// 按照下面这种格式填充
			// Mf1 -Mf2		=> 		-pv01[0] + pv02[0] 		pv01[1] - pv02[1]
			// Mf2 Mf1		=> 		-pv01[1] + pv02[1] 		-pv01[0] + pv02[0]
			setCoefficient(realRow, hv[0], -pv01[0] + pv02[0], pv01[1] - pv02[1]);
			setCoefficient(imRow, hv[0], -pv01[1] + pv02[1], -pv01[0] + pv02[0]);

			setCoefficient(realRow, hv[1], -pv02[0], pv02[1]);
			setCoefficient(imRow, hv[1], -pv02[1], -pv02[0]);

			setCoefficient(realRow, hv[2], pv01[0], 0);
			setCoefficient(imRow, hv[2], 0, pv01[0]);
		}

		// 获得稀疏矩阵
		fA.setFromTriplets(fa.begin(), fa.end());
		fA.makeCompressed();

		A.setFromTriplets(a.begin(), a.end());
		A.makeCompressed();
	}

	void LSCMBuilder::projectFace(const Mesh::FaceHandle& face, Mesh::Point* pv, Mesh::VertexHandle* hv)
	{
		Mesh::Point v[3];

		// 记录面上的所有顶点和 (u,v,h)信息
		auto i = 0;
		auto v_it = m_mesh->fv_begin(face), v_end = m_mesh->fv_end(face);
		for (; v_it != v_end; v_it++, i++)
		{
			v[i] = m_mesh->point(*v_it);
			hv[i] = *v_it;
		}

		const auto v10 = v[1] - v[0];
		const auto v10Length = v10.length();
		const auto v20 = v[2] - v[0];

		const auto x = v10 / v10Length;

		// 这里 % 是叉积！！！！！！！！！
		const auto z = (x % v20).normalized();
		const auto y = z % x;

		// 记录三角形顶点的坐标，注意 | 是点乘！！！！！！！！！！！
		pv[0] = Mesh::Point(0, 0, 0);
		pv[1] = Mesh::Point(v10Length, 0, 0);
		pv[2] = Mesh::Point(v20 | x, v20 | y, 0);
	}
} // namespace LSCM
